/* 
 * File:   cxx_misc.h
 * Author: boyan
 *
 * Created on April 20, 2009, 5:17 PM
 */

#ifndef _CXX_MISC_H
#define	_CXX_MISC_H

#include <cstdlib>
#include <cmath>
#include <cassert>

template <class T> inline void sort2(T &v1, T &v2)
{
    if(v1 > v2)
    {
        T foo = v1;
        v1 = v2;
        v2 = foo;
    }
};

template <class T> inline void sort2(T *v)
{
    if(v[0] > v[1])
    {
        T foo = v[0];
        v[0] = v[1];
        v[1] = foo;
    }
};

template <class T> inline void sort3(T &v0, T &v1, T &v2)
{
    sort2(v0, v1);
    sort2(v1, v2);
    sort2(v0, v1);
};

template <class T> inline void sort3(T *v)
{
    sort2(v[0], v[1]);
    sort2(v[1], v[2]);
    sort2(v[0], v[1]);
};

template <class T> inline T sqr(T v)
{
    return v*v;
}

static inline double dist_l1(const double *A, const double *B)
{
    return fabs(A[0] - B[0]) + fabs(A[1] - B[1]) + fabs(A[2] - B[2]);
}

static inline double dist_l2_2(const double *A, const double *B)
{
    return sqr(A[0] - B[0]) + sqr(A[1] - B[1]) + sqr(A[2] - B[2]);
}

static inline double dist_l2(const double *A, const double *B)
{
    return sqrt(sqr(A[0] - B[0]) + sqr(A[1] - B[1]) + sqr(A[2] - B[2]));
}

static inline double length(const double *A)
{
    return sqrt(sqr(A[0]) + sqr(A[1]) + sqr(A[2]));
}

static inline double length2(const double *A)
{
    return sqr(A[0]) + sqr(A[1]) + sqr(A[2]);
}

template <class T, int S> class fixed_size_vector_with_int_codes
{
public:
    T values[S];
    int codes[S];
public:

    fixed_size_vector_with_int_codes(T *to_assign)
    {
        for(size_t i = 0; i < S; i++)
        {
            values[i] = to_assign[i];
            codes[i] = 0;
        }
    }

    inline fixed_size_vector_with_int_codes & operator =(T *to_assign)
    {
        for(size_t i = 0; i < S; i++)
            values[i] = to_assign[i];
        return *this;
    }

    inline T & operator [](size_t idx)
    {
        assert(idx < S);
        return values[idx];
    }

    inline const T & operator [](size_t idx) const
    {
        assert(idx < S);
        return values[idx];
    }

    inline size_t set_value_code(T &value, int code)
    {
        for(size_t i = 0; i < S; i++)
            if(values[i] == value)
            {
                codes[i] = code;
                return i;
            }
        return S;
    }

    inline void set_idx_code(size_t idx, int code)
    {
        assert(idx < S);
        codes[idx] = code;
    }

    inline int &get_idx_code(size_t idx)
    {
        assert(idx < S);
        return codes[idx];
    }
};

#endif	/* _CXX_MISC_H */

